///////////////
//  by 辜勇胜 //
///////////////
#include "step_motor.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "Arduino.h"
// 定义电机速度 数字越小越快
int MOTOR_STEP_RATE = 5;
// 定义步进电机每次转动的步数 最大为64（360°）
int MOTOR_MAX_STEP = 4;
// 当前转动步数
int MOTOR_STEP_TMP = 0;
// 记录当前用户操作 0 -> 暂停  1 -> 正转 2 -> 反转
static int STEP_MOTOR_OPERATION = 0;


// 任务句柄
TaskHandle_t step_motor_run_task_handler = NULL;
// 任务函数
void step_motor_task(void* param);

// 初始化针脚电平
void gpio_init();

// 正转驱动
void cw_driver();
// 反转驱动
void ccw_driver();


// 初始化函数
void step_motor_init() {
  // 定义针脚模式
  pinMode(STEP_PIN_1, OUTPUT);
  pinMode(STEP_PIN_2, OUTPUT);
  pinMode(STEP_PIN_3, OUTPUT);
  pinMode(STEP_PIN_4, OUTPUT);
  // 初始化针脚电平
  gpio_init();
  // 创建异步任务
  xTaskCreate(&step_motor_task, "step_motor_task", 6 * 1024, NULL, 1, &step_motor_run_task_handler);
  // 将任务挂起
  vTaskSuspend(step_motor_run_task_handler);
}

// 正转函数
void motor_step_cw() {
  STEP_MOTOR_OPERATION = 1;
  MOTOR_STEP_TMP = 0;
  vTaskResume(step_motor_run_task_handler);
}
// 反转函数
void motor_step_ccw() {
  STEP_MOTOR_OPERATION = 2;
  MOTOR_STEP_TMP = 0;
  vTaskResume(step_motor_run_task_handler);
}
// 暂停函数
void motor_step_pause() {
  STEP_MOTOR_OPERATION = 0;
}
// 减速函数
void motor_step_sub_speed() {
  // 数值越大，转速越慢
  MOTOR_STEP_RATE++;
}
// 加速函数
void motor_step_add_speed() {
  // 数值越小，转速越快
  MOTOR_STEP_RATE--;
}

// 任务函数
void step_motor_task(void* param) {

  // 任务函数，必须是死循环
  while (1) {
    MOTOR_STEP_TMP++;
    switch (STEP_MOTOR_OPERATION) {
      case 0:
        MOTOR_STEP_TMP = 0;
        // 初始化针脚电平
        gpio_init();
        // 暂停当前任务 --> 暂停脉冲输出 --> 暂停电机转动
        vTaskSuspend( NULL );
        break;
      case 1:
        if (MOTOR_STEP_TMP > MOTOR_MAX_STEP) {
          MOTOR_STEP_TMP = 0;
          vTaskSuspend( NULL );
        }
        cw_driver();
        break;
      case 2:
        if (MOTOR_STEP_TMP > MOTOR_MAX_STEP) {
          MOTOR_STEP_TMP = 0;
          vTaskSuspend( NULL );
        }
        ccw_driver();
        break;
      default:
        break;
    }
  }
}

// 初始化针脚电平
void gpio_init() {
  digitalWrite(STEP_PIN_1, LOW);
  digitalWrite(STEP_PIN_2, LOW);
  digitalWrite(STEP_PIN_3, LOW);
  digitalWrite(STEP_PIN_4, LOW);
}

// 正转驱动 --> 参照无线四项电机时序图
void cw_driver() {
  digitalWrite(STEP_PIN_1, HIGH);
  digitalWrite(STEP_PIN_2, LOW);
  digitalWrite(STEP_PIN_3, LOW);
  digitalWrite(STEP_PIN_4, LOW);
  vTaskDelay(MOTOR_STEP_RATE);

  digitalWrite(STEP_PIN_1, HIGH);
  digitalWrite(STEP_PIN_2, HIGH);
  digitalWrite(STEP_PIN_3, LOW);
  digitalWrite(STEP_PIN_4, LOW);
  vTaskDelay(MOTOR_STEP_RATE);

  digitalWrite(STEP_PIN_1, LOW);
  digitalWrite(STEP_PIN_2, HIGH);
  digitalWrite(STEP_PIN_3, LOW);
  digitalWrite(STEP_PIN_4, LOW);
  vTaskDelay(MOTOR_STEP_RATE);

  digitalWrite(STEP_PIN_1, LOW);
  digitalWrite(STEP_PIN_2, HIGH);
  digitalWrite(STEP_PIN_3, HIGH);
  digitalWrite(STEP_PIN_4, LOW);
  vTaskDelay(MOTOR_STEP_RATE);

  digitalWrite(STEP_PIN_1, LOW);
  digitalWrite(STEP_PIN_2, LOW);
  digitalWrite(STEP_PIN_3, HIGH);
  digitalWrite(STEP_PIN_4, LOW);
  vTaskDelay(MOTOR_STEP_RATE);

  digitalWrite(STEP_PIN_1, LOW);
  digitalWrite(STEP_PIN_2, LOW);
  digitalWrite(STEP_PIN_3, HIGH);
  digitalWrite(STEP_PIN_4, HIGH);
  vTaskDelay(MOTOR_STEP_RATE);

  digitalWrite(STEP_PIN_1, LOW);
  digitalWrite(STEP_PIN_2, LOW);
  digitalWrite(STEP_PIN_3, LOW);
  digitalWrite(STEP_PIN_4, HIGH);
  vTaskDelay(MOTOR_STEP_RATE);

  digitalWrite(STEP_PIN_1, HIGH);
  digitalWrite(STEP_PIN_2, LOW);
  digitalWrite(STEP_PIN_3, LOW);
  digitalWrite(STEP_PIN_4, HIGH);
  vTaskDelay(MOTOR_STEP_RATE);
}

// 反转驱动 --> 参照无线四项电机时序图
void ccw_driver() {
  digitalWrite(STEP_PIN_1, HIGH);
  digitalWrite(STEP_PIN_2, LOW);
  digitalWrite(STEP_PIN_3, LOW);
  digitalWrite(STEP_PIN_4, HIGH);
  vTaskDelay(MOTOR_STEP_RATE);

  digitalWrite(STEP_PIN_1, LOW);
  digitalWrite(STEP_PIN_2, LOW);
  digitalWrite(STEP_PIN_3, LOW);
  digitalWrite(STEP_PIN_4, HIGH);
  vTaskDelay(MOTOR_STEP_RATE);

  digitalWrite(STEP_PIN_1, LOW);
  digitalWrite(STEP_PIN_2, LOW);
  digitalWrite(STEP_PIN_3, HIGH);
  digitalWrite(STEP_PIN_4, HIGH);
  vTaskDelay(MOTOR_STEP_RATE);

  digitalWrite(STEP_PIN_1, LOW);
  digitalWrite(STEP_PIN_2, LOW);
  digitalWrite(STEP_PIN_3, HIGH);
  digitalWrite(STEP_PIN_4, LOW);
  vTaskDelay(MOTOR_STEP_RATE);

  digitalWrite(STEP_PIN_1, LOW);
  digitalWrite(STEP_PIN_2, HIGH);
  digitalWrite(STEP_PIN_3, HIGH);
  digitalWrite(STEP_PIN_4, LOW);
  vTaskDelay(MOTOR_STEP_RATE);

  digitalWrite(STEP_PIN_1, LOW);
  digitalWrite(STEP_PIN_2, HIGH);
  digitalWrite(STEP_PIN_3, LOW);
  digitalWrite(STEP_PIN_4, LOW);
  vTaskDelay(MOTOR_STEP_RATE);

  digitalWrite(STEP_PIN_1, HIGH);
  digitalWrite(STEP_PIN_2, HIGH);
  digitalWrite(STEP_PIN_3, LOW);
  digitalWrite(STEP_PIN_4, LOW);
  vTaskDelay(MOTOR_STEP_RATE);

  digitalWrite(STEP_PIN_1, HIGH);
  digitalWrite(STEP_PIN_2, LOW);
  digitalWrite(STEP_PIN_3, LOW);
  digitalWrite(STEP_PIN_4, LOW);
  vTaskDelay(MOTOR_STEP_RATE);
}
